#include "camera_receiver.h"

#include "muduo/base/CountDownLatch.h"

#include <fcntl.h>
#include <stdio.h>

#include <linux/videodev2.h>
#include <sys/ioctl.h>

using namespace sensor::camera;

#define PIX_FORMAT(FMT)          \
    {                            \
        #FMT, V4L2_PIX_FMT_##FMT \
    }

std::unordered_map<std::string, int> CameraReceiver::camera_pix_format_table_ = {
    PIX_FORMAT(YUYV),
    PIX_FORMAT(YVYU),
    PIX_FORMAT(VYUY),
    PIX_FORMAT(UYVY),
    PIX_FORMAT(GREY),
    PIX_FORMAT(ABGR32),
    PIX_FORMAT(MJPEG),
    PIX_FORMAT(YUV420M),
};

CameraReceiver::CameraReceiver(const std::string& rxer, std::string dev, muduo::net::EventLoop* loop,
    size_t width, size_t height, const std::string& fmt)
    : logger_(rclcpp::get_logger(rxer))
    , video_dev_(std::move(dev))
    , loop_(loop)
    , width_(width)
    , height_(height)
{
    auto iter = camera_pix_format_table_.find(fmt);
    pix_format_ = (iter != camera_pix_format_table_.end()) ? iter->second : V4L2_PIX_FMT_UYVY;

    if (iter == camera_pix_format_table_.end()) {
        RCLCPP_WARN(logger_, "device %s receive unknown pix format %s! fall to UYVY!", video_dev_.c_str(), fmt.c_str());
    }

    for (auto& item : camera_pix_format_table_) {
        RCLCPP_INFO(logger_, "camera_pix_format_table_  %s == %d", item.first.c_str(), item.second);
    }
}

CameraReceiver::~CameraReceiver()
{
    disable();
}

bool CameraReceiver::enable()
{
    if (enabled_) {
        RCLCPP_INFO(logger_, "device %s has been enabled!", video_dev_.c_str());
        return true;
    }

    const int fd = open(video_dev_.c_str(), O_RDWR);
    if (fd < 0) {
        RCLCPP_ERROR(logger_, "device %s open failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }

    chnl_ = std::make_unique<muduo::net::Channel>(loop_, fd);
    enabled_ = chnl_ && getCameraCapcity() && setCameraFormat() && getCameraFormat()
        && prepareBuffers() && startVideoCapture();

    if (!enabled_) {
        RCLCPP_INFO(logger_, "device %s init failure!", video_dev_.c_str());
        releaseChannel();
        return false;
    }

    chnl_->setReadCallback(std::bind(&CameraReceiver::onReceiveOneFrame, this));
    loop_->runInLoop([&] { chnl_->enableReading(); });

    RCLCPP_INFO(logger_, "device %s init done!", video_dev_.c_str());
    return true;
}

void CameraReceiver::disable()
{
    if (!enabled_) {
        return;
    }
    stopVideoCapture();
    releaseBuffers();
    releaseChannel();
    enabled_ = false;
}

bool CameraReceiver::getCameraCapcity()
{
    struct v4l2_fmtdesc fmtdesc;
    memset(&fmtdesc, 0x00, sizeof(fmtdesc));
    fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(chnl_->fd(), VIDIOC_ENUM_FMT, &fmtdesc) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_ENUM_FMT failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }
    RCLCPP_INFO(logger_, "device %s: index = %d, %s\n", video_dev_.c_str(), fmtdesc.index, fmtdesc.description);
    return true;
}

bool CameraReceiver::setCameraFormat()
{
    struct v4l2_format format;
    memset(&format, 0x00, sizeof(format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width = width_;
    format.fmt.pix.height = height_;
    format.fmt.pix.pixelformat = pix_format_;
    format.fmt.pix.field = V4L2_FIELD_INTERLACED;
    if (ioctl(chnl_->fd(), VIDIOC_S_FMT, &format) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_S_FMT pix_format_(%d) failure! %s", video_dev_.c_str(),
            pix_format_, strerror(errno));
        return false;
    }
    return true;
}

bool CameraReceiver::getCameraFormat()
{
    struct v4l2_format format;
    memset(&format, 0x00, sizeof(format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(chnl_->fd(), VIDIOC_G_FMT, &format) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_G_FMT failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }

    RCLCPP_INFO(logger_, "device %s: width=%d height=%d fmt=%d\n", video_dev_.c_str(),
        format.fmt.pix.width, format.fmt.pix.height, format.fmt.pix.pixelformat);
    if (format.fmt.pix.width != width_ || format.fmt.pix.height != height_ || format.fmt.pix.pixelformat != pix_format_) {
        RCLCPP_WARN(logger_, "device %s desired format is not supported! width=%d height=%d fmt=%d\n",
            video_dev_.c_str(), width_, height_, pix_format_);
        width_ = format.fmt.pix.width;
        height_ = format.fmt.pix.height;
        pix_format_ = format.fmt.pix.pixelformat;
    }

    struct v4l2_streamparm streamparm;
    memset(&streamparm, 0x00, sizeof(streamparm));
    streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    ioctl(chnl_->fd(), VIDIOC_G_PARM, &streamparm);
    RCLCPP_INFO(logger_, "Camera ouput format: (%d x %d)  stride: %d, imagesize: %d, frate: %u / %u",
        width_, height_, format.fmt.pix.bytesperline, format.fmt.pix.sizeimage,
        streamparm.parm.capture.timeperframe.denominator, streamparm.parm.capture.timeperframe.numerator);

    return true;
}

bool CameraReceiver::startVideoCapture()
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(chnl_->fd(), VIDIOC_STREAMON, &type) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_STREAMON failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }
    usleep(200);
    return true;
}

bool CameraReceiver::stopVideoCapture()
{
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(chnl_->fd(), VIDIOC_STREAMOFF, &type) < 0) {
        RCLCPP_ERROR(logger_, "device %s ioctl VIDIOC_STREAMOFF failure! %s", video_dev_.c_str(), strerror(errno));
        return false;
    }
    return true;
}

void CameraReceiver::setOneFrameCallback(const OneFrameCallback& cb)
{
    cb_ = cb;
}

void CameraReceiver::releaseChannel()
{
    muduo::CountDownLatch cdl(1);
    loop_->runInLoop([&]() { chnl_->disableAll();  cdl.countDown(); });
    cdl.wait();
    close(chnl_->fd());
    chnl_ = nullptr;
}
